function out = my_iterative_cgV3( varargin )
% out = iterative_soft_thresholding[Y,angles,truncate,imgSize,maxit,intialGuessStr,constThreshValue,identifier]
%
% authors: Sebastian Schmelcher, Juergen Frikel
% version: 2012-05-10
%
% Sparse recovery: Accelerated iterated soft thresholding (AIST) with
%                  steplength selection and adapted universal threshold
%
% INPUT:
%
%   Y                   [noisy] projection Data 
%   
%   angles              measurement angles
%   
%   imgSize             (optional) size of the image to be reconstructed
%
%   maxit               (optional) max. number of iteration
%
%   initialGuessStr     (optional) initial guess
%
%   constThreshValue    regularization (thresholding) parameter
%
%
% OUTPUT variable 'out' is a struct array containing the following entries
%
%   rec                 reconstructed image
%
%   visibleCurveletIdx  indices of visible curvelets at the angular range
%                       given by 'angles'
%
%   iterates            cell array containing all iterates
%
%   error               vector of data errors of the corresponding iterates
%      
%
% Literature:
%   I. Loris, M. Bertero, C. De Mol, R. Zanella, L. Zanni. "Accelerating 
%   gradient projection methods for 1-constrained signal recovery by 
%   steplength selection rules", Appl. Comput. Harmon. Anal. 27 (2009),
%   247-254.


% fprintf(1,'Main Routine: Accelerated iterated soft thresholding (AIST)\n\n');
% fprintf(1,'\nProjection of CURVELET coefficients\n\n');

% --------------- LOOKUP TABLE for l^p-thresholding -----------------------
global x_thresh y_thresh p_thresh

x_thresh = linspace(-10,10,1000);
y_thresh = [];
p_thresh = 1.1;
% -------------------------------------------------------------------------

% intialize output variable
fprintf('checkpoint1');
out = struct( 'rec',[],'visibleCurveletIdx',[],'iterates',[],'error',[],'rectime',[] );

% Main computation
% [coeffs,ccindex,err,iterates,N] = iterate_with_steplengthSelectionForGP( varargin{:} );
tic;

identifier=varargin{7};
[coeffs,out.error,out.iterates,N] = iterative_softthresholding( identifier,varargin{1:end-1}  );

%%out.rec     = ifdct( coeffs, out.visibleCurveletIdx, N, N);
out.rec     = ifdct_wrapping( coeffs,false);%%,out.visibleCurveletIdx, N, N);
out.rectime = toc;

% sec = round(toc);
% s   = mod(sec,60);
% m   = (sec-s)/60;
% fprintf(1,'Elapsed Time: %2d min %2d sec\n\n',m,s);

%==========================================================================
function z = addCurveletCoeffs2( xfactor, x, yfactor, y )
% compute: z = xfactor*x + yfactor*y


lx = length(x);
ly = length(y);

if( lx~=ly )
    error('Coeff lengths must be equal!');
end

% allocate memory for the resulting coeff array
z = x;

% add approximation coeffs
% % if( size(x{1}{1})~=size(y{1}{1}) )
% %     error('Coeffs must have equal coeff sizes');
% % else
% %     z{1}{1} = x{1}{1} + factor.*y{1}{1};
% % end

for k=2:lx
   llx = length( x{k} );
   lly = length( y{k} );
   
   if( llx~=lly )
       error('Coeffs must have equal sizes');
   end
   
       for n=1:llx
          if( size(x{k}{n})~=size(y{k}{n}) )
              error('Coeffs must have equal coeff sizes');
          end

          z{k}{n} = xfactor.*x{k}{n} + yfactor.*y{k}{n};
       end

end

z{1}{1}=xfactor.*x{1}{1}+yfactor.*y{1}{1};

%==========================================================================
%==========================================================================
function z = addCurveletCoeffs( x, factor, y )
% compute: z = x + factor*y

lx = length(x);
ly = length(y);

if( lx~=ly )
    error('Coeff lengths must be equal!');
end

% allocate memory for the resulting coeff array
z = x;

% add approximation coeffs
if( size(x{1}{1})~=size(y{1}{1}) )
    error('Coeffs must have equal coeff sizes');
else
    z{1}{1} = x{1}{1} + factor.*y{1}{1};
end

for k=2:lx
   llx = length( x{k} );
   lly = length( y{k} );
   
   if( llx~=lly )
       error('Coeffs must have equal sizes');
   end
   
   for n=1:length(x{k})
      if( size(x{k}{n})~=size(y{k}{n}) )
          error('Coeffs must have equal coeff sizes');
      end
      
      z{k}{n} = x{k}{n} + factor.*y{k}{n};
   end
end;



% z{end}{1} = x{end}{1} + factor.*y{end}{1};

%==========================================================================
function z = multiplyCurveletCoeffs( x, y )
% compute: z = x*y


lx = length(x);
ly = length(y);

if( lx~=ly )
    error('Coeff lengths must be equal!');
end

% allocate memory for the resulting coeff array
z = x;

% add approximation coeffs
% % if( size(x{1}{1})~=size(y{1}{1}) )
% %     error('Coeffs must have equal coeff sizes');
% % else
% %     z{1}{1} = x{1}{1} + factor.*y{1}{1};
% % end

for k=1:lx-1
   llx = length( x{k} );
   lly = length( y{k} );
   
   if( llx~=lly )
       error('Coeffs must have equal sizes');
   end
   
       for n=1:llx
          if( size(x{k}{n})~=size(y{k}{n}) )
              error('Coeffs must have equal coeff sizes');
          end

          z{k}{n} = x{k}{n}.*y{k}{n};
       end

end

z{end}=x{end}.*y{end};
%==========================================================================

function sigma = estimate_std( coeffs )


tmp = [];
for n=1:length(coeffs{end-2}) %ccindex{end}
    tmp = [tmp; abs(coeffs{end-2}{n}(:))];
end

sigma = median(tmp)/0.6745;
%sigma = median(tmp(tmp~=0))/0.6745;
fprintf(1,'\nfinest scale coefficients: min = %e\tmax = %e\tmedian = %e\tsigma = %e\n',min(tmp),max(tmp),median(tmp),sigma);

%==========================================================================


function threshSeq = getThresholdingSequence( x, dimRed )

sigma  = (1-dimRed)*estimate_std( x );

if( sigma==0 )
    disp('NO TRESHOLDING!!!');
end

l1norm = 0;
maxscale = length(x);
threshSeq = cell(size(x));
for k=1:length(x)-1
    %thres = 2^((k-maxscale)*3/4)*sigma;
    thres = 2^((k-maxscale))*sigma;
    for n=1:length(x{k})
        lambda = thres*sqrt(2*log(numel(x{k}{n})));
        %lambda = thres^2*sqrt(2*log(numel(x{k}{n})));
        
        threshSeq{k}{n} = lambda;

        x{k}{n} = x{k}{n}.* (abs(x{k}{n}) > lambda);
        x{k}{n} = x{k}{n} - sign(x{k}{n}).*lambda;
        
        l1norm = l1norm + sum( sum( abs(x{k}{n}) ) );
    end
end

%==========================================================================

function [x,l1norm] = softThresh_curvelet( x, threshSeq )

l1norm = 0;
for k=1:length(x)-1
    for n=1:length(x{k})
        x{k}{n} = x{k}{n}.* (abs(x{k}{n}) > threshSeq{k}{n});
        x{k}{n} = x{k}{n} - sign(x{k}{n}).* threshSeq{k}{n};
        
        l1norm = l1norm + sum( sum( abs(x{k}{n}) ) );
    end
end

%==========================================================================

function [x,l1norm] = softThresh_curvelet_const( x, lambda )

l1norm = 0;
for k=1:length(x)-1
    for n=1:length(x{k})
        x{k}{n} = x{k}{n}.* (abs(x{k}{n}) > lambda);
        x{k}{n} = x{k}{n} - sign(x{k}{n}).* lambda;
        
        l1norm = l1norm + sum( sum( abs(x{k}{n}) ) );
    end
end


%==========================================================================

function [x,l1norm] = generic_softThresh_curvelet( x, dimRed )

sigma  = (1-dimRed)*estimate_std( x );

if( sigma==0 )
    disp('NO TRESHOLDING!!!');
end

l1norm = 0;
maxscale = length(x);
for k=1:length(x)-1
    %thres = 2^((k-maxscale)*3/4)*sigma;
    thres = 2^((k-maxscale))*sigma;
    for n=1:length(x{k})
        lambda = thres*sqrt(2*log(numel(x{k}{n})));
        %lambda = thres^2*sqrt(2*log(numel(x{k}{n})));

        x{k}{n} = x{k}{n}.* (abs(x{k}{n}) > lambda);
        x{k}{n} = x{k}{n} - sign(x{k}{n}).*lambda;
        
        l1norm = l1norm + sum( sum( abs(x{k}{n}) ) );
    end
end

%==========================================================================
function vector = extractCoeffs( coeffs )
% extract curvelet coeffs to vector

% approximation coeffs
vector = coeffs{end}(:);

% visible detail coeffs
for k=1:length(coeffs)-1
    for n=1:length(coeffs{k})
        vector = [vector;coeffs{k}{n}(:)];
    end
end

% function n = nofCurveletCoeffs( C )
% 
% n = C{end};
% for k=1:length(C)-1
%     for l=1:length(C{k})
%         n = n + numel(C{k}{l});
%     end
% end

%==========================================================================
function sos = sum_of_squared( coeffs )
% implements sum(coeffs.^2)

% approximation coeffs
sos =0;

% visible detail coeffs
for k=2:length(coeffs)
    for n=1:length(coeffs{k})
        sos = sos + sum(coeffs{k}{n}(:).^2);
    end
end

 
        sos = sos + sum(coeffs{1}{1}(:).^2);


%==========================================================================
function sop = sum_of_product( coeffs1, coeffs2 )
% implements sum(coeffs.*coeffs2)

% approximation coeffs
sop =0;

% visible detail coeffs
if(length(coeffs1)~=length(coeffs2))
    error('Coeff lengths must be equal!');
end
for k=2:length(coeffs1)
    for n=1:length(coeffs1{k})
        if(length(coeffs1{k})~=length(coeffs2{k}))
             error('Coeff lengths must be equal!');
        end
        sop = sop + sum(coeffs1{k}{n}(:).*coeffs2{k}{n}(:));
    end
end

sop = sop + sum(coeffs1{1}{1}(:).*coeffs2{1}{1}(:));

% =========================================================================
function [wNew,err,iterates,N] = iterative_softthresholding( identifier, varargin )

global exit_flag

% parse inputs
[Y,angles,imgSize,maxit,intialGuessStr,constThreshValue] = parseinputs( varargin{:} );

% global Parameters for iteration
tol   = 0.01;   % tolerance

exit_flag    = false; % exit main routine on user 
plotIterates = true; % plot each iterate?
plotError    = false;
printLog     = true;
genericThreshold = false;%true;%
constThreshold = true;%false;%



N = imgSize;
if( isempty(N) )
    initialGuess = iradon(Y,angles);
    N = max(size(initialGuess));
end

fprintf('checkpoint2');
switch lower(intialGuessStr)
    case 'zero'
        initialGuess = zeros(N);
    case 'fbp'
        initialGuess = iradon(Y,angles,N);
    case 'sfbp'
        p=Y;
        W=get_anglWeight(angles);
        for i=1:size(p,2)
            p(:,i)=p(:,i)*W(i);    
        end
        initialGuess = iradon(p,angles,N);
    case 'backprojection'
        initialGuess = backproject(Y,angles,N);
    otherwise
        error('Unknown option for the initial guess!');
end

nofPr = size(Y,1);

tv_weight = 0.001*(max(initialGuess(:))-min(initialGuess(:)));


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Initializations
wNew = fdct_wrapping( initialGuess);


dimRed =0;% degreeOfDimReduction(N,N,ccindex,identifier);

threshSeq = [];
if( ~genericThreshold && ~constThreshold)
    threshSeq = getThresholdingSequence( wNew, dimRed );
end

% compute residual
img = ifdct_wrapping( wNew,false);
proj = radon(img,angles,nofPr);
res  = norm( Y(:) - proj(:) )^2;

% Parameters of GPSS
a_min = 10e-10;
a_max = 10e+10;%10e+10;
beta  = 10e-4;
gamma = 0.5;    % = theta
nofEl = 10;     % = M
tau   = 0.5;
M_alpha = 2;
alpha_BB2_vec = inf*ones(1,M_alpha+1);

% vector of the last residuals
resvec = zeros( 1, nofEl );


    
it = 1;
if( plotIterates )
    figHandle = figure('KeyPressFcn',@keylistener);
    set(figHandle,'Position',[50 200 500 500])
    
    imagesc( real(img) );
    colormap gray;
    title(strcat('Iteration ',num2str(it)));
    drawnow;
    
    if( plotError )
        plotHandle = figure;
        set(plotHandle,'Position',[600 200 500 500])
    end
end

res  = norm( Y(:) - proj(:) )^2;
err=zeros(maxit);
err( it ) = sqrt(res);
resvec(mod(it,nofEl)+1) = res;
iterates=zeros(imgSize,imgSize,maxit);
iterates(:,:,it)=initialGuess;


b=fdct_wrapping(backproject(Y,angles,N));
%b=ct(iradon(Y,angles,N),identifier);
r_old=fdct_wrapping(backproject(radon(ifdct_wrapping(wNew,false),angles,nofPr),angles,N));
r_old=addCurveletCoeffs(r_old,-1,b);
r_new=r_old;
p=addCurveletCoeffs(r_new,-2,r_new);


fprintf('checkpoint');
while( true && it<maxit+1 )
    
    it = it + 1;
    tic;
    alpha_hat=fdct_wrapping(backproject(radon(ifdct_wrapping(p,false),angles,nofPr),angles,N));
    timespan1=toc;
    tic;
    %alpha_k=sum(extractCoeffs(r_old).^2)/sum(extractCoeffs(multiplyCurveletCoeffs(p,alpha_hat)));
    alpha_k=sum_of_squared(r_old)/sum_of_product(p,alpha_hat);
    wNew=addCurveletCoeffs(wNew,alpha_k,p);
    
    r_new=addCurveletCoeffs(r_old,alpha_k,alpha_hat);
    clear('alpha_hat');
    beta=sum_of_squared(r_new)/sum_of_squared(r_old);
    
    p=addCurveletCoeffs2(-1,r_new,beta,p); %-1*r_new+beta*p
    timespan2=toc;
    fprintf(['timespan cruvlet: ' num2str(timespan1) ' coef manipulation: ' num2str(timespan2)]);
    r_old=r_new;
   
 
    l1norm=0;

    % Soft-thresholding: h = S( xOld -alpha*grad(xOld) );
%     if( genericThreshold )
%         [wNew,l1norm] = generic_softThresh_curvelet( wNew, 0 );
%     elseif( constThreshold )
%         [wNew,l1norm] = softThresh_curvelet_const( wNew, constThreshValue );
%     else
%         [wNew,l1norm] = softThresh_curvelet( wNew, threshSeq );
%     end


    %compute current error
    img = ifdct_wrapping( wNew, false);
    proj = radon(img,angles,nofPr);
    res  = norm( Y(:) - proj(:) )^2;

    err( it ) = sqrt(res);
    resvec(mod(it,nofEl)+1) = res;

    
    
    % new function value
    fval = res + l1norm;
    
    img=real(img);
    if(mod(it,1)==0)
        if( plotIterates )
            figure(figHandle);
            imagesc( img );
            colormap gray;
            title(strcat('Iteration ',num2str(it)));
            drawnow;

            if( plotError )
                figure(plotHandle)
                semilogy(it,res,'x');
                xlabel('iterations');
                ylabel('absolute error');
                hold on;
            end

        end
        if( printLog )
            fprintf(1,'it=%5d;\terr=%e;\tfval=%e;\tl1-norm=%e;\tstep=%e;\ttv_weight = %e;\n',it,err(it),fval,l1norm,alpha_k,tv_weight);
        end
    end

    
        
    % save current iterate
    iterates(:,:,it) = img;

    if( exit_flag )
        return;
    end
end

%shorten preallocated vectors
err=err(1:it);


%B = real( ifdct( wNew, ccindex, N, N ) );

if( printLog )
    %fprintf(1,'it=%5d;\terr=%e;\tfval=%e;\tl1-norm=%e;\tstep=%e;\tdiff=%e;\n',it,err(it),fval,l1norm,alpha,diff);
    fprintf(1,'Eof Main Computation\n\n');
end
%Eof

% =========================================================================
function [Y,angles,imgSize,maxit,intialGuessStr,constThreshValue] = parseinputs( varargin )

narginchk(2,6);

if( nargin>=1 ) 
    Y = varargin{1};
end

if( nargin>=2 ) 
    angles = varargin{2};
end


imgSize = [];
if( nargin>=3 ) 
    imgSize = varargin{3};
end

maxit = 50;
if( nargin>=4 ) 
    maxit = varargin{4};
end
    
intialGuessStr = 'zero';
if( nargin>=5 )
    if( ~ischar(varargin{5}) )
        error('Initial guess parameter must be a string!');
    end
    intialGuessStr = varargin{5};
end

constThreshValue = 1;
if( nargin>=6 )
    constThreshValue = varargin{6};
end


% =========================================================================
function keylistener(src,eventdata)

global exit_flag

if strcmp(eventdata.Key,'x')
    exit_flag = true;
    disp('Exit Main routine');
end
